// 实时发送关节角度信息
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "trajectory_msgs/JointTrajectory.h"
#include "sensor_msgs/JointState.h"

void Callback(const sensor_msgs::JointStateConstPtr& msg)
{
    float pos[5] = {0};
    int len = msg->position.size();  // 一段轨迹有多少个路径点
    for (int j = 0; j < len && j < 5; j++) {  // 确保不会越界
        pos[j] = msg->position[j];
    }
    if (len >= 5) {  // 只有当我们确实有5个或更多的位置数据时，才打印信息
        ROS_INFO("pos,%f,%f,%f,%f,%f", pos[0], pos[1], pos[2], pos[3], pos[4]);
    }
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "listener"); //listener 
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("/joint_states", 10000, Callback); //
    ros::Rate rate(100);
    ros::spin();
    return 0;
}

